/*
    This source file is part of GeNa project.
    Copyright © GeNa project 2008
    For the latest info, see http://code.google.com/p/genaproject/ or  http://lumpyproject.forums-rpg.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Foobar is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar.  If not, see <http://www.gnu.org/licenses/>. */

#include <cmath>

#include "free_camera.h"
#include "application.h"
#include "unique_name_generator.h"

#include <GL/gl3.h>

void FreeCamera::init()
{
    mat = math::matrix4f::Identity;

    propMgrInstance.addProperty(this, "FreeCamera", "mouseSensiX", 0.035f, false, &FreeCamera::m_sensibilityX);
    propMgrInstance.addProperty(this, "FreeCamera", "mouseSensiY", 0.035f, false, &FreeCamera::m_sensibilityY);

    propMgrInstance.addProperty(this, "FreeCamera", "moveSensiX", 0.1f, false, &FreeCamera::m_moveSensiX);
    propMgrInstance.addProperty(this, "FreeCamera", "moveSensiY", 0.1f, false, &FreeCamera::m_moveSensiY);
    propMgrInstance.addProperty(this, "FreeCamera", "moveSensiUP", 0.1f, false, &FreeCamera::m_moveSensiUp);

    propMgrInstance.addProperty(this, "FreeCamera", "pitchMax", 1.55f, false, &FreeCamera::m_pitchMax);

    propMgrInstance.addProperty(this, "FreeCamera", "yaw", 0.0f, true, &FreeCamera::m_yaw);
    propMgrInstance.addProperty(this, "FreeCamera", "pitch", 0.0f, true, &FreeCamera::m_pitch);
    propMgrInstance.addProperty(this, "FreeCamera", "roll", 0.0f, true, &FreeCamera::m_roll);

    propMgrInstance.addProperty(this, "FreeCamera", "x", 0.0f, true, &FreeCamera::m_x);
    propMgrInstance.addProperty(this, "FreeCamera", "y", 0.0f, true, &FreeCamera::m_y);
    propMgrInstance.addProperty(this, "FreeCamera", "z", 0.0f, true, &FreeCamera::m_z);
}

FreeCamera::FreeCamera()
{
    init();
}


FreeCamera::FreeCamera(const FreeCamera &copy)
{
    init();

    m_x = copy.m_x;
    m_y = copy.m_y;
    m_z = copy.m_z;

    m_yaw = copy.m_yaw;
    m_pitch = copy.m_pitch;
    m_roll = copy.m_roll;

    mat = copy.mat;
}

FreeCamera& FreeCamera::operator=(const FreeCamera &copy)
{
    m_x = copy.m_x;
    m_y = copy.m_y;
    m_z = copy.m_z;

    m_yaw = copy.m_yaw;
    m_pitch = copy.m_pitch;
    m_roll = copy.m_roll;

    mat = copy.mat;
    return (*this);
}

FreeCamera::~FreeCamera()
{
    propMgrInstance.removeObj(this);
}

void FreeCamera::moveTo( math::vector3f pos)
{
    m_x = pos.x;
    m_y = pos.y;
    m_z = pos.z;
}

void FreeCamera::addAbsoluteDelta(math::vector3f pos)
{
    m_x += pos.x;
    m_y += pos.y;
    m_z += pos.z;
}

void FreeCamera::addRelatifDelta(math::vector3f pos)
{
    updateCamera();
    m_x += mat[0][0]*pos.x + mat[1][0]*pos.y - mat[2][0]*pos.z;
    m_y += mat[0][2]*pos.x + mat[1][2]*pos.y - mat[2][2]*pos.z;
    m_z += mat[0][1]*pos.x + mat[1][1]*pos.y - mat[2][1]*pos.z;

    updateCamera();
}

void FreeCamera::addKeyboardMove( int forward, int right, int up)
{
    addRelatifDelta(math::vector3f(m_moveSensiX*right, m_moveSensiUp*up, m_moveSensiY*forward));
}


void FreeCamera::setOrientation( float yaw, float pitch, float roll)
{
    updateCamera();
}


void FreeCamera::addToOrientation( float yaw, float pitch, float roll)
{


    m_yaw += yaw; m_pitch += pitch; m_roll += roll;

    updateCamera();
}

void FreeCamera::updateCamera()
{

    if( m_pitch > m_pitchMax)
        m_pitch = m_pitchMax;

    if( m_pitch < -m_pitchMax)
        m_pitch = -m_pitchMax;

    const float cr = cos( m_roll );
    const float sr = sin( m_roll );
    const float cp = cos( m_pitch);
    const float sp = sin( m_pitch );
    const float cy = cos( m_yaw );
    const float sy = sin( m_yaw );


    const float srsp = sr*sp;
    const float srcp = sr*cp;

    math::matrix4f mat2(1, 0, 0, -m_x,
                        0, 1, 0, -m_z,
                        0, 0, 1, -m_y,
                        0, 0, 0, 1);
    mat = math::matrix4f(
                        cr*cy,          -cr*sy,         -sr,    0, //m_z*cr*cy           - m_x*cr*sy           - m_y*sr,
                        srcp*cy+sp*sy,  -srcp*sy+sp*cy, cp*cr,  0, //m_z*(srcp*cy+sp*sy) - m_x*(srcp*sy+sp*cy) + m_y*cp*cr,
                        srsp*cy-cp*sy,  -srsp*sy-cp*cy, sp*cr,  0, //m_z*(srsp*cy-cp*sy) - m_x*(srsp*sy-cp*cy) + m_y*sp*cr,
                        0,              0,              0,      1);
    mat *= mat2;
}

void FreeCamera::addMouseOrientation( float mouse_x, float mouse_y)
{
    addToOrientation(m_sensibilityX*mouse_x, m_sensibilityY*mouse_y, 0);
}

math::quaternionf FreeCamera::eulerToQuat( float yaw, float pitch, float roll)
{
    float phi2 = yaw/2;
    float theta2 = pitch/2;
    float psi2 = roll/2;

    float cos_phi = cos(phi2);
    float cos_theta = cos(theta2);
    float cos_psi = cos(psi2);

    float sin_phi = sin(phi2);
    float sin_theta = sin(theta2);
    float sin_psi = sin(psi2);

    math::quaternionf quat;
    quat.w = cos_phi*cos_theta*cos_psi - sin_phi*sin_theta*sin_psi;
    quat.x = sin_phi*cos_theta*cos_psi + cos_phi*sin_theta*sin_psi;
    quat.y = cos_phi*sin_theta*cos_psi - sin_phi*cos_theta*sin_psi;
    quat.z = cos_phi*cos_theta*sin_psi + sin_phi*sin_theta*cos_psi;

    return quat;
}

void FreeCamera::quatToEuler( math::quaternionf quat, float &yaw, float &pitch, float &roll)
{
    float sin_pitch = 2*(quat.w*quat.y - quat.z*quat.x);
    //Gimbal lock special case :
    if( sin_pitch >= 1)
    {
        yaw = 2 * atan2(quat.x, quat.w);
        pitch = M_PI_2;
        roll = 0;
    }

    if( sin_pitch <= 1)
    {
        yaw = -2 * atan2(quat.x, quat.w);
        pitch = -M_PI_2;
        roll = 0;
    }

    pitch = asin(sin_pitch);
    roll = atan2( 2*(quat.w*quat.x + quat.y*quat.z), 1-2*(quat.x*quat.x + quat.y*quat.y));
    yaw = atan2( 2*(quat.w*quat.z - quat.x*quat.y), 1-2*(quat.y*quat.y + quat.z*quat.z));
    return;
}

void FreeCamera::apply()
{
    updateCamera();
    glMatrixMode(GL_MODELVIEW);
    glLoadTransposeMatrixf(mat.l);
}

